#include "mergesort_inversion_vector.h"
#include <string.h>
#include <stdlib.h>

unsigned int *make_inversion_vector2(const unsigned int *seq, unsigned int len, unsigned int max_value)
{
	unsigned int *data = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *temp = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *sorted_inversions = (unsigned int *)calloc(max_value, sizeof(unsigned int));

	memcpy(data, seq, sizeof(unsigned int) * len);
	compute_sorted_inversions(data, sorted_inversions, temp, 0, len);

	int i;
	for(i=0; i<len; i++) {
		data[i] = sorted_inversions[seq[i]];
	}

	free(sorted_inversions);
	free(temp);

	return data;
}

unsigned int *make_inversion_vector(const unsigned int *seq, unsigned int len)
{
	return make_inversion_vector2(seq, len, len);
}

unsigned int *make_reverse_inversion_vector2(const unsigned int *seq, unsigned int len, unsigned int max_value)
{
	unsigned int *inversions = make_inversion_vector2(seq, len, max_value);
	int i;

	for(i=0; i<len; i++) {
		inversions[i] = seq[i] - (i - inversions[i]);
	}

	return inversions;
}

unsigned int *make_reverse_inversion_vector(const unsigned int *seq, unsigned int len)
{
	return make_reverse_inversion_vector2(seq, len, len);
}

void insertion_sort(unsigned int *data, unsigned int *sorted_inversions, unsigned int begin, unsigned int end)
{
	int i, j;
	for(i=begin+1; i<end; i++) {
		unsigned int tmp = data[i];
		for(j=i; j>begin && tmp < data[j-1]; j--) {
			sorted_inversions[tmp]++;
			data[j] = data[j-1];
		}
		data[j] = tmp;
	}
}

void compute_sorted_inversions(unsigned int *data, unsigned int *sorted_inversions, unsigned int *temp, unsigned int begin, unsigned int end)
{
	if(end - begin <= 16) {
		insertion_sort(data, sorted_inversions, begin, end);
		return;
	}

	unsigned int mid = (begin + end) / 2;
	compute_sorted_inversions(data, sorted_inversions, temp, begin, mid);
	compute_sorted_inversions(data, sorted_inversions, temp, mid, end);

	if(begin < mid && data[mid - 1] < data[mid]) {
		return;
	}

	// optimize by skipping over data that is already in the right place
	while(begin < mid && data[begin] <= data[mid]) {
		begin++;
	}

	unsigned int i=0, p=begin, q=mid;
	while(p < mid && q < end) {
		if(data[q] < data[p]) {
			sorted_inversions[data[q]] += mid - p;
			temp[i++] = data[q++];
		}
		else {
			temp[i++] = data[p++];
		}
	}

	if(p < mid) {
		// optimize by moving this remaining initial segment directly to the end, rather than passing through temp
		memcpy(data + begin + i, data + p, (mid - p) * sizeof(unsigned int));
	}

	memcpy(data + begin, temp, i * sizeof(unsigned int));
}

unsigned int *invert_inversion_vector(const unsigned int *inversions, unsigned int len)
{
	unsigned int *inversions_copy = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *temp = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *index = (unsigned int *)malloc(sizeof(unsigned int) * len);

	memcpy(inversions_copy, inversions, sizeof(unsigned int) * len);
	int i;
	for(i=0; i<len; i++) {
		index[i] = i;
	}
	invert_inversion_vector_recursive(inversions_copy, index, temp, 0, len);

	for(i=0; i<len; i++) {
		inversions_copy[index[i]] = i;
	}

	free(index);
	free(temp);

	return inversions_copy;
}

unsigned int *invert_reverse_inversion_vector(const unsigned int *rsiv, unsigned int len)
{
	unsigned int *inversions = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *temp = (unsigned int *)malloc(sizeof(unsigned int) * len);
	unsigned int *index = (unsigned int *)malloc(sizeof(unsigned int) * len);
	int i;

	for(i=0; i<len; i++) {
		index[i] = i;
	}
	for(i=0; i<len; i++) {
		// flip backwards so the same function can be used
		inversions[i] = rsiv[len - i - 1];
	}

	invert_inversion_vector_recursive(inversions, index, temp, 0, len);

	for(i=0; i<len; i++) {
		// invert the permutation
		temp[index[i]] = i;
	}

	for(i=0; i<len; i++) {
		// flip back to normal & correct
		inversions[i] = len - temp[len - i - 1] - 1;
	}

	free(index);
	free(temp);

	return inversions;
}

void invert_inversion_insertion_sort(unsigned int *inversions, unsigned int *index, unsigned int begin, unsigned int end)
{
	int i, j;
	for(i=begin+1; i<end; i++) {
		unsigned int tmp = index[i];
		for(j=i; j>begin && inversions[tmp] > inversions[index[j-1]]; j--) {
			inversions[tmp]--;
			index[j] = index[j-1];
		}
		index[j] = tmp;
	}
}

void invert_inversion_vector_recursive(unsigned int *inversions, unsigned int *index, unsigned int *temp, unsigned int begin, unsigned int end)
{
	if(end - begin <= 16) {
		invert_inversion_insertion_sort(inversions, index, begin, end);
		return;
	}

	unsigned int mid = (begin + end) / 2;
	invert_inversion_vector_recursive(inversions, index, temp, begin, mid);
	invert_inversion_vector_recursive(inversions, index, temp, mid, end);

	if(begin < mid && inversions[index[mid - 1]] >= inversions[index[mid]]) {
		return;
	}

	// optimize by skipping over data that is already in the right place
	while(begin < mid && inversions[index[begin]] >= inversions[index[mid]]) {
		begin++;
	}

	unsigned int i=0, p=begin, q=mid;
	while(p < mid && q < end) {
		if((inversions[index[q]] > inversions[index[p]]) && (inversions[index[q]] - inversions[index[p]] >= mid - p)) {
			inversions[index[q]] -= mid - p;
			temp[i++] = index[q++];
		}
		else {
			temp[i++] = index[p++];
		}
	}

	if(p < mid) {
		memcpy(index + begin + i, index + p, (mid - p) * sizeof(unsigned int));
	}

	memcpy(index + begin, temp, i * sizeof(unsigned int));
}
